from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, OpaqueFunction
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
import os

def print_move_group(context, *args, **kwargs):
    # 使用提供的上下文解析 LaunchConfiguration 的值
    move_group_value = LaunchConfiguration('move_group').perform(context)
    print("move_group:", move_group_value)
    return []

def generate_launch_description():
    # 声明 move_group 参数
    move_group_arg = DeclareLaunchArgument(
        'move_group',
        default_value='zc4',
        description='Move group name for Servo node'
    )

    moveit_config_pkg = get_package_share_directory('zc4_moveit_config')
    ros2_control_pkg = get_package_share_directory('zc4_moveit_config')
    urdf_pkg = get_package_share_directory('test_urdf')  
    
    # 配置文件路径
    urdf_file = os.path.join(urdf_pkg, 'urdf', 'test_urdf.urdf')
    srdf_file = os.path.join(moveit_config_pkg, 'config', 'test_urdf.srdf')
    moveit_servo_config = os.path.join(moveit_config_pkg, 'config', 'moveit_servo.yaml')
    controllers_config = os.path.join(ros2_control_pkg, 'config', 'controllers.yaml')

    # 读取 URDF 和 SRDF
    robot_description = {"robot_description": open(urdf_file, 'r').read()}
    robot_description_semantic = {"robot_description_semantic": open(srdf_file, 'r').read()}

    # 获取 move_group 参数（LaunchConfiguration对象）
    move_group = LaunchConfiguration('move_group')

    servo_node = Node(
        package='moveit_servo',
        executable='servo_node_main',
        name='moveit_servo',
        output='screen',
        parameters=[
            moveit_servo_config,
            robot_description,
            robot_description_semantic,
            {'moveit_servo.move_group_name': move_group}
        ]
    )

    # ros2_control 部分保持不变
    from launch.launch_description_sources import PythonLaunchDescriptionSource
    from launch.actions import IncludeLaunchDescription
    ros2_control_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(os.path.join(ros2_control_pkg, 'launch', 'ros2_control.launch.py')),
        launch_arguments={'controllers_config': controllers_config}.items()
    )

    ld = LaunchDescription()
    ld.add_action(move_group_arg)
    ld.add_action(OpaqueFunction(function=print_move_group))  # 调试：打印 move_group 值
    ld.add_action(servo_node)
    ld.add_action(ros2_control_launch)

    return ld
